
#include <stdlib.h>
#include <string.h>
#include <stdio.h>

#include "dualfifo.h"


static void dualfifo_calculateCopyRegions( dualfifo_t *fifo, unsigned int pointer, unsigned int size,
		unsigned int * tailBytes, unsigned int * headBytes );
static int dualfifo_getPointers( dualfifo_t *fifo, dualfifo_mode mode,
		 unsigned int **startPointer, unsigned int **endPointer );

int dualfifo_new( dualfifo_t * fifo, unsigned int initialSize, unsigned int maximumSize )
{
	fifo->readPointer = 0;
	fifo->parsePointer = 0;
	fifo->writePointer = 0;
	
	// Ensure initialSize is a power of 2
	initialSize = initialSize & (~0x01);
	
	fifo->buffer = malloc( initialSize * sizeof( char ) );
	fifo->bufferSize = initialSize;
	fifo->maximumSize = maximumSize;
	
	if ( fifo->buffer == NULL )
	{
		return -1;
	}
	else
	{
		return initialSize;
	}
}

void dualfifo_free( dualfifo_t *fifo )
{
	if( fifo->buffer != NULL )
	{
		free( fifo->buffer );
		
		memset( fifo, 0, sizeof( dualfifo_t ) );
	}
	else
	{
		#ifdef DEBUG
			printf( __FILE__ ": double free detected\n" );
		#endif
	}
}

static void dualfifo_calculateCopyRegions( dualfifo_t *fifo, unsigned int pointer, unsigned int size,
		unsigned int * tailBytes, unsigned int * headBytes )
{
	*tailBytes = fifo->bufferSize - pointer;
	
	if( *tailBytes >= size )
	{
		// Easy ... the data can be moved in one operation
		*tailBytes = size;
		*headBytes = 0;
	}
	else
	{
		*headBytes = size - *tailBytes;
	}
}

static int dualfifo_getPointers( dualfifo_t *fifo, dualfifo_mode mode,
		 unsigned int **startPointer, unsigned int **endPointer )
{
	switch( mode )
	{
		case DUALFIFO_MODE_WRITE:
			*startPointer = &fifo->writePointer;
			*endPointer = &fifo->parsePointer;
			break;
		case DUALFIFO_MODE_READ:
			*startPointer = &fifo->readPointer;
			*endPointer = &fifo->parsePointer;
			break;
		case DUALFIFO_MODE_PARSE:
			*startPointer = &fifo->parsePointer;
			*endPointer = &fifo->writePointer;
			break;
		case DUALFIFO_MODE_BOTH:
			if( fifo->parsePointer != fifo->readPointer )
			{
				return -1;
			}
			
			*startPointer = &fifo->parsePointer;
			*endPointer = &fifo->writePointer;
			break;
		default:
			return -1;
	}
	
	return 0;
}

int dualfifo_bytesAvailable( dualfifo_t *fifo, dualfifo_mode mode )
{
	unsigned int available = 0;
	
	unsigned int *startPointer, *endPointer;
	
	if( dualfifo_getPointers( fifo, mode, &startPointer, &endPointer ) < 0 )
	{
		return 0;
	}
	
	if( fifo->bufferSize == 0 )
	{
		return 0;
	}
	
	available = *endPointer - *startPointer;
	
	if( mode == DUALFIFO_MODE_WRITE )
	{
		available -= 1;
		
		if( fifo->maximumSize > fifo->bufferSize )
		{
			return available % fifo->maximumSize;
		}
		else
		{
			return available % fifo->bufferSize;
		}
	}
	
	return available % fifo->bufferSize;
}

int dualfifo_write( dualfifo_t *fifo, const void * data, unsigned int size )
{
	unsigned int available = 0;

	// Check if it's possible to fit data into buffer
	if( dualfifo_bytesAvailable( fifo, DUALFIFO_MODE_WRITE ) <= size )
	{
		return 0;
	}
	
	// Do we need to expand our buffer?
	available = (fifo->readPointer - (fifo->writePointer + 1)) % fifo->bufferSize;
	if( size >= available )
	{
		unsigned int newSize;
		void * newBuffer;
		
		newSize = fifo->bufferSize - available + size;
		printf( "Debug: available %i, size %i, buffer %i, new %i\n", available, size, fifo->bufferSize, newSize );
		
		// Round to multiple of bufferSize
		newSize = (newSize / fifo->bufferSize  + 2 ) * fifo->bufferSize;
		printf( "Debug: newSize %i\n", newSize );
		
		// Cannot fit into limits requested (maximumSize <= 0 means no limit)
		if( newSize > fifo->maximumSize && fifo->maximumSize > 0 )
		{
			return 0;
		}
		
		newBuffer = realloc( fifo->buffer, newSize );
		if( newBuffer == NULL )
		{
			return 0;
		}
		
		fifo->buffer = newBuffer;
		fifo->bufferSize = newSize;
	}
	
	// At this point, wildly assume that the data will fit into the FIFO
	{
		unsigned int tailBytes = 0;
		unsigned int headBytes = 0;
		
		dualfifo_calculateCopyRegions( fifo, fifo->writePointer, size, &tailBytes, &headBytes );
		
		memcpy( fifo->buffer + fifo->writePointer, data, tailBytes );
		if( headBytes > 0 )
		{
			memcpy( fifo->buffer, data + tailBytes, headBytes );
		}
		
		fifo->writePointer += size;
		fifo->writePointer = fifo->writePointer % fifo->bufferSize;
		
		return size;
	}
		
	return 0;
}


int dualfifo_peek( dualfifo_t *fifo,  dualfifo_mode readMode, void * data, unsigned int size )
{
	unsigned int *startPointer, *endPointer;
	unsigned int bytesAvailable;
	unsigned int headBytes, tailBytes;
	
	if( dualfifo_getPointers( fifo, readMode, &startPointer, &endPointer ) < 0 )
	{
		return 0;
	}
	
	if( readMode == DUALFIFO_MODE_WRITE )
	{
		return 0;
	}
	
	bytesAvailable = dualfifo_bytesAvailable( fifo, readMode );
	if ( bytesAvailable < size )
	{
		size = bytesAvailable;
	}
	
	dualfifo_calculateCopyRegions( fifo, *startPointer, size,
		&tailBytes, &headBytes );
		
	memcpy( data, fifo->buffer + *startPointer, tailBytes );
	if( headBytes > 0 )
	{
		memcpy( data + tailBytes, fifo->buffer, headBytes );
	}
	
	return size;
}

int dualfifo_read( dualfifo_t *fifo,  dualfifo_mode readMode, void * data, unsigned int size )
{

	unsigned int bytesCopied; 
	
	bytesCopied = dualfifo_peek( fifo, readMode, data, size );
	
	if( bytesCopied > 0 )
	{
		dualfifo_advance( fifo, readMode, bytesCopied );
	}
	
	return bytesCopied;
}

int dualfifo_advance( dualfifo_t *fifo,  dualfifo_mode readMode, unsigned int size )
{
	unsigned int *startPointer, *endPointer;
	
	if( readMode == DUALFIFO_MODE_WRITE )
	{
		return 0;
	} 
	
	if( dualfifo_bytesAvailable( fifo, readMode ) < size )
	{
		return 0;
	}
	
	dualfifo_getPointers( fifo, readMode, &startPointer, &endPointer );
	
	*startPointer += size;
	*startPointer = *startPointer % fifo->bufferSize;
	
	if( readMode == DUALFIFO_MODE_BOTH )
	{
		fifo->readPointer = fifo->parsePointer;
	}
	
	return size;
}

